#ifndef NODE_HPP
#define NODE_HPP

#include <eigen3/Eigen/Core>
#include "rclcpp/rclcpp.hpp"
#include <vector>
#include <queue>
#include <limits>

namespace astar_planner
{
    const float sqrt2 = 1.414;
    const float sqrt2_2 = 0.414;

    const float UNKNOWN = 255;          // 未知状态
    const float OCCUPIED = 254;         // 被占据状态
    const float INSCRIBED = 253;        // 内切障碍状态
    const float MAX_NON_OBSTACLE = 252; // 非障碍物的最大值
    const float FREE = 0;               // 空闲状态

    typedef unsigned char COSTTYPE;

    enum class LISTYPE
    {
        UNKNOWN = 0,
        OPENLIST,
        CLOSELIST,
    };

    class Node;
    typedef Node *NodePtr;
    class NodeComparator;

    typedef std::vector<Node> Graph;
    typedef std::priority_queue<NodePtr,std::vector<NodePtr>,NodeComparator> NodeQueue;     // 总代价值小的在顶端
    typedef std::vector<int> NeighborsModelVector;

    class Node
    {
    private:
        unsigned int index_;
        float cell_cost_;
        Eigen::Vector2i coords_;

        float f_x; // 总代价
        float g_x; // 累积代价
        float h_x; // 启发函数

        LISTYPE type_;

        float weight_g_;
        float weight_h_;

    public:
        Node(unsigned int index, int i, int j, float cost) : index_(index), parent_(nullptr)
        {
            coords_(0) = i; // x
            coords_(1) = j; // y
            cell_cost_ = cost;
            f_x = g_x = std::numeric_limits<float>::max();
            h_x = 0.0;
            type_ = LISTYPE::UNKNOWN;
            weight_g_ = 1.0;
            weight_h_ = 1.0;
        }
        ~Node() = default;

        inline bool operator==(const NodePtr &rhs)
        {
            return index_ == rhs->index_;
        }

        inline int get_index() const
        {
            return index_;
        }

        inline float get_cost() const
        {
            return cell_cost_;
        }
        inline void set_cost(const float &cost)
        {
            cell_cost_ = cost;
        }

        // 判断是否在closelist中
        inline bool was_visited()
        {
            return type_ == LISTYPE::CLOSELIST;
        }
        inline void visited()
        {
            type_ = LISTYPE::CLOSELIST;
        }

        // 判断是否在openlist中
        inline bool was_queued()
        {
            return type_ == LISTYPE::OPENLIST;
        }
        inline void queued()
        {
            type_ = LISTYPE::OPENLIST;
        }

        // 判断是否未知
        inline bool was_unknown()
        {
            return type_ == LISTYPE::UNKNOWN;
        }

        // 获取坐标
        inline Eigen::Vector2i get_coord()
        {
            return coords_;
        }
        // 获取x坐标
        inline float get_coord_x()
        {
            return coords_(0);
        }
        // 获取y坐标
        inline float get_coord_y()
        {
            return coords_(1);
        }
        // 配置坐标
        void set_coordinate(int i, int j)
        {
            coords_(0) = i;
            coords_(1) = j;
        }

        float F_x() const
        {
            return f_x;
        }
        float G_x() const
        {
            return g_x;
        }
        float H_x() const
        {
            return h_x;
        }

        void update(float g, float h)
        {
            g_x = g;
            h_x = h * weight_h_;
            f_x = g_x + h_x;
        }
        NodePtr parent_;
    };

    // 仿函数类，比较两个节点代价大小
    class NodeComparator
    {
    public:
        // 返回true表示a的总代价值大，false为b
        bool operator()(const NodePtr &a, const NodePtr &b) const
        {
            return a->F_x() > b->F_x();
        }
    };

    // 枚举类，枚举运动模型
    enum class MotionModel
    {
        UNKNOWN = 0,     // 未知运动模型
        VON_NEUMANN = 1, // 冯·诺伊曼邻域运动模型----上下左右
        MOORE = 2,       // 摩尔邻域运动模型----上下左右+右上、右下、左上、左下
        DUBIN = 3,       // Dubins 曲线运动模型
        REEDS_SHEPP = 4, // Reeds-Shepp 曲线运动模型
    };

    inline std::string to_string(const MotionModel &n)
    {
        switch (n)
        {
        case MotionModel::VON_NEUMANN:
            return "Von neumann";
        case MotionModel::MOORE:
            return "Moore";
        case MotionModel::DUBIN:
            return "Dubin";
        case MotionModel::REEDS_SHEPP:
            return "Reeds-Shepp";
        default:
            return "Unknown";
        }
    }
    inline MotionModel from_string(const std::string &n)
    {
        if (n == "VON_NEUMANN")
        {
            return MotionModel::VON_NEUMANN;
        }   
        else if (n == "MOORE")
        {
            return MotionModel::MOORE;
        }
        else if (n == "DUBIN")
        {
            return MotionModel::DUBIN;
        } 
        else if (n == "REEDS_SHEPP")
        {
            return MotionModel::REEDS_SHEPP;
        } 
        else
        {
            return MotionModel::UNKNOWN;
        }     
    }

} // namespace astar_planner

#endif // NODE_HPP